import numpy as np


# 最初六自由度时候安装需要坐标变换，现在不需要了
def sensor_to_site(force_sensor = np.zeros(6)):

    # # R01 means ^{0}R_{1}, and ^{0}P = ^{0}R_{1} * ^{1}P
    # # 1 means sensor frame, 0 means robot frame
    # R01 = np.array([
    #     [0, -1, 0],
    #     [0, 0, -1],
    #     [1, 0, 0]
    # ])

    # site_force = R01.dot(force_sensor[:3])
    # # print(f"site force is {site_force}")
    # site_torque = R01.dot(force_sensor[3:])
    # # print(f"site torque is {site_torque}")
    # return np.concatenate((site_force, site_torque))
    return force_sensor